#include "matrix.h"
#include <R_ext/Applic.h>

void
CAR1_recalc(double *Xy, int *pdims, int *ZXcol,
	    double *par, double *time, double *logdet)
{
    int N = pdims[0], M = pdims[1], *len = pdims + 4, *start = len + M, i;
    double aux = exp(*par);
    /* parameter assumed in unconstrained form */
    *par = aux / (1.0 + aux);
    for(i = 0; i < M;  i++) 
    {
	      double *Factor = Calloc(len[i] * len[i], double);
	      CAR1_fact(par, time + start[i], &len[i], Factor, logdet);
	      mult_mat(Xy + start[i], N, Factor, len[i], len[i], len[i], Xy + start[i], N, *ZXcol);
	      Free(Factor);
    }
}

static void
CAR1_fact(double *par, double *time, int *n, double *mat, double *logdet)
{
    int job = 11L, info, i, nsq = *n * (*n), np1 = *n + 1;
    double *work = Calloc(*n, double), *work1 = Calloc(nsq, double);
    CAR1_mat(par, time, n, mat);
    F77_CALL(chol) (mat, n, n, mat, &info);
    for(i = 0; i < *n; i++) 
    {
	      work1[i * np1] = 1;
	      F77_CALL(dtrsl) (mat, n, n, work1 + i * (*n), &job, &info);
	      *logdet -= log(fabs(mat[i * np1]));
    }
    Memcpy(mat, work1, nsq);
    Free(work); Free(work1);
}


double *
mult_mat(double *z, int ldz,
	 double *x, int ldx, int xrows, int xcols,
	 double *y, int ldy, int ycols)
{                /* z <- x %*% y */
  double *t, *tmp = Calloc((size_t)(xrows * ycols), double);
  int i, j;            /* use tmp so z can be either x or y */

  t = tmp;
  for (i = 0; i < ycols; i++) {
    for (j = 0; j < xcols; j++) {
      d_axpy(t, y[j], x + j * ldx, xrows);
    }
    t += xrows;
    y += ldy;
  }
  copy_mat(z, ldz, tmp, xrows, xrows, ycols);
  Free(tmp);
  return z;
}


static void
CAR1_mat(double *par, double *time, int *n, double *mat)
{
    int i, j;
    double aux;
    for(i = 0; i < *n; i++) {
	*(mat + i * (*n + 1)) = 1.0;
	for(j = i + 1; j < *n; j++) {
	    aux = pow(*par, fabs(time[j] - time[i]));
	    *(mat + i + j * (*n)) = *(mat + j + i * (*n)) = aux;
	}
    }
}


void
d_axpy(double *y, double a, double *x, int n)
{                /* y <- a * x + y  */
  while (n-- > 0) { *y++ += a * *x++; }
}


double *
copy_mat(double *y, int ldy, double *x, int ldx, int nrow, int ncol)
{                /* y <- x */
  double * ret = y;
  while (ncol-- > 0) { Memcpy(y, x, nrow); y += ldy; x += ldx; }
  return ret;
}

